
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

// for GPS
#include <SoftwareSerial.h>

MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL

bool dmpReady = false;  // set true if DMP init was successful
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// Serial Setup from GPS Module - Dx Pins (example: D3+D4)
SoftwareSerial serialGPS(3, 4); // RX, TX

// Serial Setup to remzibi - Dx Pins (example: D6+D7)
SoftwareSerial remzibi(6, 7); // RX, TX


char readBuffer[300];
int gpsLine = -1;
String gpsLines[2];


// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // initialize serial communication
  Serial.begin(9600);
  
  //remzibi
  remzibi.begin(9600);

  //GPS Setup
  serialGPS.begin(9600); // last one is listening

  mpu.initialize();

  // load and configure the DMP
  //Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {

  if (!dmpReady) {
    Serial.println(F("DMP not ready"));
    return;
  }

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if (fifoCount == 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    //Serial.println(F("FIFO overflow!"));
  } else  {

    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    // 1 = roll  2 = nick
    double roll = (double) ypr[1] * 180 / M_PI;
    double nick = (double) ypr[2] * 180 / M_PI;

    int roll_rnd = (int) roll;
    int nick_rnd = (int) nick;

    printAH(nick_rnd, roll_rnd);

    //GPS
    doGPS();

  }
}


void doGPS() {
  // GPS Read here
  if (serialGPS.overflow()) {
    serialGPS.println("SoftwareSerial overflow!");
    serialGPS.flush();
  }

  for (int run = 0; run < 200; run++) {
    while (!serialGPS.available()) {} // hang on until data is avalable
    readBuffer[run] = serialGPS.read();
  }

  gpsLines[0] = "";
  gpsLines[1] = "";
  bool print = false;
  
  // iterate over read data
  for (int run = 0; run < 180; run++) {
    String last = String(readBuffer[run]);

    if (last == "$" && run < 225) {
      print = false;
      String key = String(readBuffer[run]) + String(readBuffer[run + 1]) + String(readBuffer[run + 2]) + String(readBuffer[run + 3]) + String(readBuffer[run + 4]) + String(readBuffer[run + 5]);

      //$GPGGA,153450.00,4749.03532,N,01238.30368,E,1,06,2.56,649.2,M,45.2,M,,*58
      if (key == "$GPGGA") {
        gpsLine = 0;
        print = true;
      }
      //$GPRMC,153448.00,A,4749.03535,N,01238.30364,E,1.039,,101014,,,A*75
      if (key == "$GPRMC") {
        gpsLine = 1;
        print = true;
      }
    }
    if (print == true) {
      if (last == "\n") {
        remzibi.println(gpsLines[gpsLine]);
        Serial.println(gpsLines[gpsLine]);
        gpsLines[gpsLine] = "";
      } else {
        gpsLines[gpsLine] = gpsLines[gpsLine] + last;
      }
    }
  }

}

// PAL
// middle line 8
// top line 5
// bottom line 11
// cols  9,11,13 -O- 18, 20, 22
void printAH(int nick, int roll) {
  printAHRow(1, nick, roll);
  printAHRow(2, nick, roll);
  printAHRow(3, nick, roll);
  printAHRow(4, nick, roll);
  printAHRow(5, nick, roll);
  printAHRow(6, nick, roll);
  printAHRow(7, nick, roll);
}

void printAHRow(int row, int nick, int roll) {
  int nickoffset = getNickoffset(nick);
  int rollfactor = getRollfactor(roll);

  char rowChrs[14] = {' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' ', ' '};
  if (row == (nickoffset + round(1 * (rollfactor)) + 4)) rowChrs[0] = '-';

  if (row == (nickoffset + round(0.8 * (rollfactor)) + 4)) rowChrs[1] = '-';
  if (row == (nickoffset + round(0.7 * (rollfactor)) + 4)) rowChrs[2] = '-';

  if (row == (nickoffset + round(0.5 * (rollfactor)) + 4)) rowChrs[3] = '-';
  if (row == (nickoffset + round(0.4 * (rollfactor)) + 4)) rowChrs[4] = '-';

  if (row == (nickoffset + round(0.3 * (rollfactor)) + 4)) rowChrs[5] = '-';
  if (row == (nickoffset + round(0.2 * (rollfactor)) + 4)) rowChrs[6] = '-';

  if (row == (nickoffset - round(0.2 * (rollfactor)) + 4)) rowChrs[7] = '-';
  if (row == (nickoffset - round(0.3 * (rollfactor)) + 4)) rowChrs[8] = '-';

  if (row == (nickoffset - round(0.4 * (rollfactor)) + 4)) rowChrs[9] = '-';
  if (row == (nickoffset - round(0.5 * (rollfactor)) + 4)) rowChrs[10] = '-';

  if (row == (nickoffset - round(0.7 * (rollfactor)) + 4)) rowChrs[11] = '-';
  if (row == (nickoffset - round(0.8 * (rollfactor)) + 4)) rowChrs[12] = '-';

  if (row == (nickoffset - round(1 * (rollfactor)) + 4)) rowChrs[13] = '-';

  if (row == 4) {
    rowChrs[6] = '(';
    rowChrs[7] = ')';
  }

  //$M09080000+rowChars
  int displayrow = row + 5;
  String rowstring = String(displayrow);
  if (displayrow < 10) rowstring = "0" + rowstring;

  //HEX
  if (displayrow == 10) rowstring = "0A";
  if (displayrow == 11) rowstring = "0B";
  if (displayrow == 12) rowstring = "0C";
  if (displayrow == 13) rowstring = "0D";

  remzibi.print("$M09" + rowstring + "0000");
  Serial.print("$M09" + rowstring + "0000");

  for (int run = 0; run < 14; run++) {
    remzibi.print(String(rowChrs[run]));
    Serial.print(String(rowChrs[run]));
  }
  //Serial.println("nickoffset:"+String(nickoffset)+ "   rollfactor:"+String(rollfactor));
  remzibi.println();
  Serial.println();
}

int getNickoffset(int nick) {
  int nickoffset = 0;
  if (nick > 1 && nick <= 5) nickoffset = 1;
  if (nick > 5 && nick <= 10) nickoffset = 2;
  if (nick > 10) nickoffset = 3;

  if (nick < -1 && nick >= -5) nickoffset = -1;
  if (nick < -5 && nick >= -10) nickoffset = -2;
  if (nick < -10) nickoffset = -3;

  return nickoffset;
}

int getRollfactor(int roll) {
  int rollfactor = 0;
  if (roll >= 1 && roll <= 5) rollfactor = -1;
  if (roll > 5 && roll <= 10) rollfactor = -2;
  if (roll > 10) rollfactor = -3;

  if (roll <= -1 && roll >= -5) rollfactor = 1;
  if (roll < -5 && roll >= -10) rollfactor = 2;
  if (roll < -10) rollfactor = 3;

  return rollfactor;
}






